#pragma once

#include <ocs2_core/Types.h>
#include <ocs2_core/initialization/Initializer.h>
#include <ocs2_ddp/DDP_Settings.h>
#include <ocs2_mpc/MPC_Settings.h>
#include <ocs2_oc/rollout/TimeTriggeredRollout.h>
#include <ocs2_oc/synchronized_module/ReferenceManager.h>
#include <ocs2_robotic_tools/common/RobotInterface.h>

#include "mpc_quadrotor_mujoco/QuadrotorParameters.h"
#include "mpc_quadrotor_mujoco/definitions.h"

namespace ocs2 {
    namespace quadrotor {
        class QuadrotorInterface final : public RobotInterface {
        public:
            QuadrotorInterface(const std::string& taskFile, const std::string& libraryFolder);

            ~QuadrotorInterface() override = default;

            const vector_t& getInitialState() { return initialState_; }

            ddp::Settings& ddpSettings() { return ddpSettings_; }

            mpc::Settings& mpcSettings() { return mpcSettings_; }

            const OptimalControlProblem& getOptimalControlProblem() const override { return problem_; }

            std::shared_ptr<ReferenceManagerInterface> getReferenceManagerPtr() const override { return referenceManagerPtr_; }

            const RolloutBase& getRollout() const { return *rolloutPtr_; }

            const Initializer& getInitializer() const override { return *operatingPointPtr_; }

        private:
            ddp::Settings ddpSettings_;
            mpc::Settings mpcSettings_;

            OptimalControlProblem problem_;
            std::shared_ptr<ReferenceManager> referenceManagerPtr_;

            std::unique_ptr<RolloutBase> rolloutPtr_;
            std::unique_ptr<Initializer> operatingPointPtr_;

            vector_t initialState_{STATE_DIM};
        };
    } // namespace quadrotor
} // namespace ocs2

